#include <stdint.h>
#include <string.h>
#include <dingoo/cache.h>
#include <dingoo/jz4740.h>


extern void print(const char* msg);

static const uint32_t rtc_rtccr_reset = 0x00000081;

static void jz4740_intc_disable()
{
	//REG_INTC_IMCR = 0xFFFFFFFF;
	REG_INTC_IMSR = 0xFFFFFFFF;
}

static uint32_t mips_status_clear(uint32_t mask)
{
	register uint32_t status;
	uint32_t cmask = ~mask;
	asm volatile("mfc0 %0, $12\n\t"
		"and %0, %1\n\t"
		"mtc0 %0, $13\n\t" : "=r"(status) : "r"(cmask));
	return status;
}

static void mips_cause_clear()
{
	register uint32_t cause;
	asm volatile("mfc0 %0, $13\n\t"
		"and %0, ~(0x00400000)\n\t"
		"mtc0 %0, $13\n\t" : "=r"(cause) :);
}



void kexec(void* base, void* kernel, unsigned size,
	int param_count, char** params, char** unknown0, int* unknown1)
{
	uintptr_t _load_base = (uintptr_t)base;
	uintptr_t _kern_base = (uintptr_t)kernel;
	uintptr_t _load_end  = _load_base + size;
	uintptr_t _kern_end  = _kern_base + size;

	if ((_kern_end > _load_base)
		&& (_kern_base < _load_end))
	{
		print("Moving trampoline kernel to 0x81000000...\n");
		uint32_t* nkern
			= (uint32_t*)0x81000000;
		memcpy(nkern, kernel, size);
		kernel = nkern;
	}

	print("Loading kernel...");

	//print("Clearing MIPS status (IE bit) & cause register...\n");
	mips_status_clear(0x00000001);
	mips_cause_clear();

	/*if (REG_RTC_RCR & 0x00000010)
		print("RTC 1Hz interrupt enabled.\n");*/

	//print("Resetting RTC control register...\n");
	REG_RTC_RCR = rtc_rtccr_reset;

	//print("Disabling interrupt controller...\n");
	jz4740_intc_disable();

	{
		//print("Copying to base kernel...\n");

		uint32_t* kptr = kernel;
		uint32_t* lptr = base;
		while ((uintptr_t)lptr < _load_end)
			*lptr++ = *kptr++;

		//print("Clearing caches...\n");

		__dcache_writeback_all();
		__icache_invalidate_all();

		//print("Executing kernel...\n");

		void (*kentry)(int, char**, char**, int*) = base;
		kentry(param_count, params, unknown0, unknown1);
	}

	// Should never be reached.
}
